/* Siconos is a program dedicated to modeling, simulation and control
 * of non smooth dynamical systems.
 *
 * Copyright 2024 INRIA.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

/** \file NewtonEulerDS.hpp
 */

#ifndef NEWTONEULERNLDS_H
#define NEWTONEULERNLDS_H

#include "BoundaryCondition.hpp"
#include "DynamicalSystem.hpp"
#include "SecondOrderDS.hpp"

/** Pointer to function for plug-in. */
typedef void (*FInt_NE)(double t, double *q, double *v, double *f,
                        unsigned int size_z, double *z);
typedef void (*FExt_NE)(double t, double *f, unsigned int size_z, double *z);

void computeT(SP::SiconosVector q, SP::SimpleMatrix T);
#include "RotationQuaternion.hpp"

/** Compute the force and moment vectors applied to a body with state
 *  q from a force vector at a given position. */
void computeExtForceAtPos(SP::SiconosVector q,
                          bool isMextExpressedInInertialFrame,
                          SP::SiconosVector force, bool forceAbsRef,
                          SP::SiconosVector pos, bool posAbsRef,
                          SP::SiconosVector fExt, SP::SiconosVector mExt,
                          bool accumulate);

/** 
    NewtonEuler non linear dynamical systems

    The equations of motion in the Newton-Euler formalism can be stated as
    
    \f[
    \left\{\begin{array}{rcl}
    M \dot v +  F_{int}(q,v, \Omega, t)&=& F_{ext}(t), \\
    I \dot \Omega + \Omega \wedge I\Omega  + M_{int}(q,v, \Omega, t) &=&
    M_{ext}(t), \\
    \dot q &=& T(q) [ v, \Omega] \\
    \dot R &=& R \tilde \Omega,\quad R^{-1}=R^T,\quad  \det(R)=1 .
    \end{array}\right.
    \f]

    with
    
    - \f$ x_G,v_G \f$ position and velocity of the center of mass expressed in a
    inertial frame of reference (world frame)
    - \f$ \Omega \f$ angular velocity vector expressed in the body-fixed frame (frame attached to the
    object)
    - \f$ R \f$ rotation matrix form the inertial frame to the
    body-fixed frame \f$ R^{-1}=R^T, \det(R)=1 \f$, i.e \f$ R\in SO^+(3) \f$
    - \f$ M=m\,I_{3\times 3} \f$ diagonal mass matrix with  \f$ m \in \mathbb{R} \f$ the scalar mass
    - \f$ I \f$ constant inertia matrix
    - \f$ F_{ext} \f$ and \f$ M_{ext} \f$ are the external applied forces and moment

    In the current implementation, \f$ R \f$ is parametrized by a unit quaternion.

*/
class NewtonEulerDS : public SecondOrderDS {
protected:
  
  ACCEPT_SERIALIZATION(NewtonEulerDS);

  /** Common code for constructors
   * should be replaced in C++11 by delegating constructors
   */
  void _init();

  // -- MEMBERS --

  /** _twist contains the twist of the Newton Euler dynamical system.
   *  _twist[0:2] : \f$ v_G \in \RR^3 \f$ velocity of the center of mass in
   *  the inertial frame of reference (world frame).
   *  _twist[3:5] : \f$ \Omega\in\RR^3 \f$ angular velocity expressed in the
   *  body-fixed frame
   */
  SP::SiconosVector _twist;

  /** Initial twist */
  SP::SiconosVector _twist0;

  /** _q contains the representation of the system
   *   In the current implementation, we have
   *   _q[0:2] : the coordinates of the center of mass expressed
   *      in the inertial frame of reference (world frame)
   *   _q[3:6] : an unit quaternion representing the orientation of the solid.
   *      This unit quaternion encodes the rotation mapping from the inertial
   *   frame of reference to the body-fixed frame
   */
  SP::SiconosVector _q;

  /** Dimension of _q, is not necessary equal to _ndof. In our case, _qDim = 7
   * and  _ndof =6*/
  unsigned int _qDim;

  /** The time derivative of  \f$ q \f$ ,  \f$ \dot q \f$ */
  SP::SiconosVector _dotq;

  /** _acceleration contains the time derivative of the twist
   */
  SP::SiconosVector _acceleration;

  /** Memory vectors that stores the values within the time--step */
  SiconosMemory _twistMemory;
  SiconosMemory _qMemory;
  SiconosMemory _forcesMemory;
  SiconosMemory _dotqMemory;

  /** Inertial matrix
   */
  SP::SiconosMatrix _I;

  /** Scalar mass of the system
   */
  double _scalarMass;

  /** Matrix depending on the parametrization of the orientation
   * \f$ v = T(q) \dot q \f$
   */
  SP::SimpleMatrix _T;

  /** Time derivative of T.
   *
   * \f$ \dot v = \dot T(q) \dot q + T(q) \ddot q \f$
   */
  SP::SimpleMatrix _Tdot;

  /** external forces of the system */
  SP::SiconosVector _fExt;

  /** boolean if _fext is constant (set thanks to setFExtPtr for instance)
   * false by default */
  bool _hasConstantFExt;

  /** internal forces of the system */
  SP::SiconosVector _fInt;

  /** external moment expressed in the inertial frame */
  SP::SiconosVector _mExt;

  /** boolean if _mext is constant (set thanks to setMExtPtr for instance)
   * false by default */
  bool _hasConstantMExt;

  /** if true, we assume that mExt is given in inertial frame (default false) */
  bool _isMextExpressedInInertialFrame;

  /** external moment expressed in the body-fixed frame  */
  // SP::SiconosVector _mExtBodyFrame;

  /** internal moment of the forces */
  SP::SiconosVector _mInt;

  /** jacobian_q FInt  w.r.t q*/
  SP::SimpleMatrix _jacobianFIntq;

  /** jacobian_twist FInt  w.r.t the twist*/
  SP::SimpleMatrix _jacobianFInttwist;

  /** jacobian_q MInt w.r.t q */
  SP::SimpleMatrix _jacobianMIntq;

  /** jacobian_twist MInt  w.r.t the twist*/
  SP::SimpleMatrix _jacobianMInttwist;

  /** jacobian_q MExt w.r.t q*/
  SP::SimpleMatrix _jacobianMExtq;

  /** gyroscpical moment  */
  SP::SiconosVector _mGyr;

  /** jacobian_twist of mGyr w.r.t the twist*/
  SP::SimpleMatrix _jacobianMGyrtwist;

  /** wrench (q,twist,t)= [ fExt - fInt ; mExtBodyFrame - mGyr - mInt ]^T */

  SP::SiconosVector _wrench;

  /** jacobian_q forces*/
  SP::SimpleMatrix _jacobianWrenchq;

  /** jacobian_{twist} forces*/
  SP::SimpleMatrix _jacobianWrenchTwist;

  /** if true, we set the gyroscopic forces equal to 0 (default false) **/
  bool _nullifyMGyr;

  /** If true, we compute the missing Jacobian by forward finite difference */
  bool _computeJacobianFIntqByFD;

  /** If true, we compute the missing Jacobian by forward finite difference */
  bool _computeJacobianFInttwistByFD;

  /** If true, we compute the missing Jacobian by forward finite difference */
  bool _computeJacobianMIntqByFD;

  /** If true, we compute the missing Jacobian by forward finite difference */
  bool _computeJacobianMInttwistByFD;

  /** value of the step in finite difference */
  double _epsilonFD;

  /** Plugin to compute strength of external forces */
  SP::PluggedObject _pluginFExt;

  /** Plugin to compute the external moment expressed in the inertial frame  */
  SP::PluggedObject _pluginMExt;

  /** Plugin to compute strength of internal forces */
  SP::PluggedObject _pluginFInt;

  /** Plugin to compute moments of internal forces */
  SP::PluggedObject _pluginMInt;

  /** The following code is commented because the jacobian of _mInt and _fInt
   *  are not yet used by the numerical scheme.
   *  Will be needed by a fully implicit scheme for instance.
   */
  /* jacobian_q */
  //  SP::SimpleMatrix _jacobianqmInt;
  /* jacobian_{qDot} */
  //  SP::SimpleMatrix _jacobianqDotmInt;

  /** NewtonEulerDS plug-in to compute \f$ \nabla_qF_{Int}(\dot q, q, t) \f$, id =
   *  "jacobianFIntq"
   *  @param time : current time
   *  @param sizeOfq : size of vector q
   *  @param q : pointer to the first element of q
   *  @param twist : pointer to the first element of twist
   *  @param[in,out] jacob : pointer to the first element of the jacobian
   *  @param  size of vector z
   *  @param[in,out] z  : a vector of user-defined parameters
   */
  SP::PluggedObject _pluginJacqFInt;

  /** NewtonEulerDS plug-in to compute \f$ \nabla_{\dot q}F_{Int}(\dot q, q,
   *  t) \f$, id = "jacobianFIntTwist"
   *  @param time : current time
   *  @param sizeOfq : size of vector q
   *  @param q : pointer to the first element of q
   *  @param twist : pointer to the first element of twist
   *  @param[in,out] jacob : pointer to the first element of the jacobian
   *  @param  size of vector z
   * @param[in,out] z  : a vector of user-defined parameters
   */
  SP::PluggedObject _pluginJactwistFInt;

  /** NewtonEulerDS plug-in to compute \f$ \nabla_qM_{Int}(\dot q, q, t) \f$, id =
   *  "jacobianMInttwist"
   *  @param time : current time
   *  @param sizeOfq : size of vector q
   *  @param q : pointer to the first element of q
   *  @param twist : pointer to the first element of twist
   *  @param[in,out] jacob : pointer to the first element of the jacobian
   *  @param  size of vector z
   *  @param[in,out] z  : a vector of user-defined parameters
   */
  SP::PluggedObject _pluginJacqMInt;

  /** NewtonEulerDS plug-in to compute \f$ \nabla_{\dot q}M_{Int}(\dot q, q,
   *  t) \f$, id = "jacobianMInttwist"
   *  @param time : current time
   *  @param sizeOfq : size of vector q
   *  @param q : pointer to the first element of q
   *  @param twist : pointer to the first element of twist
   *  @param[in,out] jacob : pointer to the first element of the jacobian
   *  @param  size of vector z
   *  @param[in,out] z  : a vector of user-defined parameters
   */
  SP::PluggedObject _pluginJactwistMInt;

  enum NewtonEulerDSRhsMatrices {
    jacobianXBloc00,
    jacobianXBloc01,
    jacobianXBloc10,
    jacobianXBloc11,
    zeroMatrix,
    zeroMatrixqDim,
    numberOfRhsMatrices
  };

  /** A container of matrices to save matrices that are involed in first order
   *  from of NewtonEulerDS system values (jacobianXBloc10, jacobianXBloc11,
   *  zeroMatrix, idMatrix) No get-set functions at the time. Only used as a
   *  protected member.*/
  VectorOfSMatrices _rhsMatrices;

  /** Default constructor
   */
  NewtonEulerDS();

  /** build all _plugin... PluggedObject */
  void _zeroPlugin() override;

public:
  // === CONSTRUCTORS - DESTRUCTOR ===

  /** constructor from a minimum set of data
   *
   *  \param position initial coordinates of this DynamicalSystem
   *  \param twist initial twist of this DynamicalSystem
   *  \param mass the mass
   *  \param inertia the inertia matrix
   */
  NewtonEulerDS(SP::SiconosVector position, SP::SiconosVector twist,
                double mass, SP::SiconosMatrix inertia);

  /** destructor */
  virtual ~NewtonEulerDS();

  /** reset the state to the initial state */
  void resetToInitialState() override;

  /** allocate (if needed)  and compute rhs and its jacobian.
   *
   *  \param time of initialization
   */
  void initRhs(double time) override;

  /** set nonsmooth input to zero
   *
   *  \param level input-level to be initialized.
   */
  void initializeNonSmoothInput(unsigned int level) override;

  /** update right-hand side for the current state
   *
   *  \param time of interest
   */
  void computeRhs(double time) override;

  /** update \f$ \nabla_x rhs \f$ for the current state
   *
   *  \param time of interest
   */
  void computeJacobianRhsx(double time) override;

  /** reset non-smooth part of the rhs (i.e. p), for all 'levels' */
  void resetAllNonSmoothParts() override;

  /** set nonsmooth part of the rhs (i.e. p) to zero for a given level
   *
   *  \param level
   */
  void resetNonSmoothPart(unsigned int level) override;

  // -- forces --
  /** get forces
   *
   *  \return pointer on a SiconosVector
   */
  inline SP::SiconosVector forces() const override { return _wrench; }

  // -- Jacobian Forces w.r.t q --

  /** get JacobianqForces
   *
   *  \return pointer on a SiconosMatrix
   */
  inline SP::SiconosMatrix jacobianqForces() const override
  {
    return _jacobianWrenchq;
  }

  /** get JacobianvForces
   *
   *  \return pointer on a SiconosMatrix
   */
  inline SP::SiconosMatrix jacobianvForces() const override
  {
    return _jacobianWrenchTwist;
  }

  /** Returns dimension of vector q */
  virtual inline unsigned int getqDim() const { return _qDim; }

  // -- q --

  /** get q (pointer link)
   *
   *  \return pointer on a SiconosVector
   */
  inline SP::SiconosVector q() const override { return _q; }

  /** set value of generalized coordinates vector (copy)
   *
   *  \param newValue
   */
  void setQ(const SiconosVector &newValue) override;

  /** set value of generalized coordinates vector (pointer link)
   *
   *  \param newPtr
   */
  void setQPtr(SP::SiconosVector newPtr) override;

  /** set initial state (copy)
   *
   *  \param newValue
   */
  void setQ0(const SiconosVector &newValue) override;

  /** set initial state (pointer link)
   *
   *  \param newPtr
   */
  void setQ0Ptr(SP::SiconosVector newPtr) override;

  // -- twist --

  /** get twist
   *
   *  \return pointer on a SiconosVector
   */
  inline SP::SiconosVector twist() const { return _twist; }

  /** get twist
   *
   *  \return pointer on a SiconosVector
   *  this accessor is left to get a uniform access to velocity.
   *  This should be removed with MechanicalDS class
   */
  inline SP::SiconosVector velocity() const override { return _twist; }

  inline SP::SiconosVector twist0() const { return _twist0; }

  inline SP::SiconosVector velocity0() const override { return _twist0; }
  /** set  velocity (copy)
   *
   *  \param newValue
   */
  void setVelocity(const SiconosVector &newValue) override;

  /** set  velocity (pointer link)
   *
   *  \param newPtr
   */
  void setVelocityPtr(SP::SiconosVector newPtr) override;

  /** set initial velocity (copy)
   *
   *  \param newValue
   */
  void setVelocity0(const SiconosVector &newValue) override;

  /** set initial velocity (pointer link)
   *
   *  \param newPtr
   */
  void setVelocity0Ptr(SP::SiconosVector newPtr) override;

  /** get acceleration (pointer link)
   *
   *  \return pointer on a SiconosVector
   */
  SP::SiconosVector acceleration() const override { return _acceleration; };

  /** default function to compute the mass
   */
  void computeMass() override{};

  /** function to compute the mass
   *
   *  \param position value used to evaluate the mass matrix
   */
  void computeMass(SP::SiconosVector position) override{};

  /** Get the linear velocity in the absolute (inertial) or relative
   *  (body) frame of reference.
   *
   *  \param absoluteRef If true, velocity is returned in the inertial
   *  frame, otherwise velocity is returned in the body frame.
   *  \return A SiconosVector of size 3 containing the linear velocity
   *  of this dynamical system.
   */
  SP::SiconosVector linearVelocity(bool absoluteRef) const;

  /** Fill a SiconosVector with the linear velocity in the absolute
   *  (inertial) or relative (body) frame of reference.
   *
   *  \param absoluteRef If true, velocity is returned in the inertial
   *  frame, otherwise velocity is returned in the body frame.
   *  \param v A SiconosVector of size 3 to receive the linear velocity.
   */
  void linearVelocity(bool absoluteRef, SiconosVector &v) const;

  /** Get the angular velocity in the absolute (inertial) or relative
   *  (body) frame of reference.
   *
   *  \param absoluteRef If true, velocity is returned in the inertial
   *  frame, otherwise velocity is returned in the body frame.
   *  \return A SiconosVector of size 3 containing the angular velocity
   *  of this dynamical system.
   */
  SP::SiconosVector angularVelocity(bool absoluteRef) const;

  /** Fill a SiconosVector with the angular velocity in the absolute
   *  (inertial) or relative (body) frame of reference.
   *
   *  \param absoluteRef If true, velocity is returned in the inertial
   *  frame, otherwise velocity is returned in the body frame.
   *  \param w A SiconosVector of size 3 to receive the angular velocity.
   */
  void angularVelocity(bool absoluteRef, SiconosVector &w) const;

  // -- p --

  // -- Mass --

  /** get mass value
   *
   *  \return a double
   */
  inline double scalarMass() const { return _scalarMass; };

  /** Modify the scalar mass */
  void setScalarMass(double mass)
  {
    _scalarMass = mass;
    updateMassMatrix();
  };

  /** \return the inertia matrix
   */
  SP::SiconosMatrix inertia() { return _I; };

  /**
    Modify the inertia matrix (pointer link)
    
    \param newInertia the new inertia matrix
  */
  void setInertia(SP::SiconosMatrix newInertia)
  {
    _I = newInertia;
    updateMassMatrix();
  }

  /**
    Modify the inertia matrix.
    
    \param ix x component
    \param iy y component
    \param iz z component
  */
  void setInertia(double ix, double iy, double iz);

  /** to be called after scalar mass or inertia matrix have changed */
  void updateMassMatrix();

  // -- Fext --
  /** get fExt
   *
   *  \return pointer on a plugged vector
   */
  inline SP::SiconosVector fExt() const { return _fExt; }

  /** set fExt to pointer newPtr
   *
   *  \param   newPtr a SP to a Simple vector
   */
  inline void setFExtPtr(SP::SiconosVector newPtr)
  {
    _fExt = newPtr;
    _hasConstantFExt = true;
  }
	
  /** get mExt
   *
   *  \return pointer on a plugged vector
   */
   inline SP::SiconosVector mExt() const { return _mExt; }

  /** set mExt to pointer newPtr
   *
   *  \param newPtr a SP to a Simple vector
   */
  inline void setMExtPtr(SP::SiconosVector newPtr)
  {
    _mExt = newPtr;
    _hasConstantMExt = true;
  }

  /** get mGyr
   *
   *  \return pointer on a plugged vector
   */
  inline SP::SiconosVector mGyr() const { return _mGyr; }

  inline SP::SimpleMatrix T() { return _T; }
  inline SP::SimpleMatrix Tdot()
  {
    assert(_Tdot);
    return _Tdot;
  }

  inline SP::SiconosVector dotq() { return _dotq; }

  /** get all the values of the state vector q stored in memory
   *
   *  \return a memory
   */
  inline const SiconosMemory &qMemory() override { return _qMemory; }

  /** get all the values of the state vector twist stored in memory
   *
   *  \return a memory
   */
  inline const SiconosMemory &twistMemory() { return _twistMemory; }
  /** get all the values of the state vector twist stored in memory
   *
   *  \return a memory
   */
  inline const SiconosMemory &velocityMemory() override { return _twistMemory; }

  /** initialize the SiconosMemory objects with a positive size.
   *
   *  \param steps the size of the SiconosMemory (i)
   */
  void initMemory(unsigned int steps) override;

  /** push the current values of x, q and r in the stored previous values
   *  xMemory, qMemory, rMemory,
   *  \todo Modify the function swapIn Memory with the new Object Memory
   */
  void swapInMemory() override;

  inline const SiconosMemory &forcesMemory() override { return _forcesMemory; }

  inline const SiconosMemory &dotqMemory() { return _dotqMemory; }

  /** To compute the kinetic energy
   */
  double computeKineticEnergy();

  // --- miscellaneous ---

  /** print the data to the screen
   */
  void display(bool brief = true) const override;

  //  inline SP::SiconosMatrix jacobianZFL() const { return jacobianZFL; }

  inline void setIsMextExpressedInInertialFrame(bool value)
  {
    _isMextExpressedInInertialFrame = value;
    if (!_jacobianMExtq)
      _jacobianMExtq.reset(new SimpleMatrix(3, _qDim));
    if (!_jacobianWrenchq)
      _jacobianWrenchq.reset(new SimpleMatrix(_ndof, _qDim));
  }

  inline void setNullifyMGyr(bool value) { _nullifyMGyr = value; }

  virtual void normalizeq();

  /** 
      Allocate memory for the lu factorization of the mass of the system.
      Useful for some integrators with system inversion involving the mass
  */
  void init_inverse_mass() override;

  /** 
      Update the content of the lu factorization of the mass of the system,
      if required.
  */
  void update_inverse_mass() override;


  inline void setComputeJacobianFIntqByFD(bool value)
  {
    _computeJacobianFIntqByFD = value;
  }
  inline void setComputeJacobianFIntvByFD(bool value)
  {
    _computeJacobianFInttwistByFD = value;
  }
  inline void setComputeJacobianMIntqByFD(bool value)
  {
    _computeJacobianMIntqByFD = value;
  }
  inline void setComputeJacobianMIntvByFD(bool value)
  {
    _computeJacobianMInttwistByFD = value;
  }

  /** allow to set a specified function to compute _fExt
   *
   *  \param pluginPath the complete path to the plugin
   *  \param functionName the name of the function to use in this plugin
   */
  void setComputeFExtFunction(const std::string &pluginPath,
                              const std::string &functionName)
  {
    _pluginFExt->setComputeFunction(pluginPath, functionName);
    if (!_fExt)
      _fExt.reset(new SiconosVector(3, 0));
    _hasConstantFExt = false;
  }
  
  /** allow to set a specified function to compute _mExt
   *
   *  \param pluginPath the complete path to the plugin
   *  \param functionName the name of the function to use in this plugin
   */
  void setComputeMExtFunction(const std::string &pluginPath,
                              const std::string &functionName)
  {
    _pluginMExt->setComputeFunction(pluginPath, functionName);
    if (!_mExt)
      _mExt.reset(new SiconosVector(3, 0));
    _hasConstantMExt = false;
  }

  /** set a specified function to compute _fExt
   *
   *  \param fct a pointer on the plugin function
   */
  void setComputeFExtFunction(FExt_NE fct)
  {
    _pluginFExt->setComputeFunction((void *)fct);
    if (!_fExt)
      _fExt.reset(new SiconosVector(3, 0));
    _hasConstantFExt = false;
  }

  /** set a specified function to compute _mExt
   *
   *  \param fct a pointer on the plugin function
   */
  void setComputeMExtFunction(FExt_NE fct)
  {
    _pluginMExt->setComputeFunction((void *)fct);
    if (!_mExt)
      _mExt.reset(new SiconosVector(3, 0));
    _hasConstantMExt = false;
  }

  /** allow to set a specified function to compute _fInt
   *
   *  \param pluginPath the complete path to the plugin
   *  \param functionName the name of the function to use in this plugin
   */
  void setComputeFIntFunction(const std::string &pluginPath,
                              const std::string &functionName)
  {
    _pluginFInt->setComputeFunction(pluginPath, functionName);
    if (!_fInt)
      _fInt.reset(new SiconosVector(3, 0));
  }
  /** allow to set a specified function to compute _mInt
   *
   *  \param pluginPath the complete path to the plugin
   *  \param functionName the name of the function to use in this plugin
   */
  void setComputeMIntFunction(const std::string &pluginPath,
                              const std::string &functionName)
  {
    _pluginMInt->setComputeFunction(pluginPath, functionName);
    if (!_mInt)
      _mInt.reset(new SiconosVector(3, 0));
  }

  /** set a specified function to compute _fInt
   *
   *  \param fct a pointer on the plugin function
   */
  void setComputeFIntFunction(FInt_NE fct)
  {
    _pluginFInt->setComputeFunction((void *)fct);
    if (!_fInt)
      _fInt.reset(new SiconosVector(3, 0));
  }

  /** set a specified function to compute _mInt
   *
   *  \param fct a pointer on the plugin function
   */
  void setComputeMIntFunction(FInt_NE fct)
  {
    _pluginMInt->setComputeFunction((void *)fct);
    if (!_mInt)
      _mInt.reset(new SiconosVector(3, 0));
  }

  /** allow to set a specified function to compute the jacobian w.r.t q of the
   *  internal forces 
   *
   *  \param pluginPath std::string : the complete path to the plugin
   *  \param functionName std::string : the name of the function to use in this plugin
   */
  void setComputeJacobianFIntqFunction(const std::string &pluginPath,
                                       const std::string &functionName);

  /** allow to set a specified function to compute the jacobian following v of
   *  the internal forces w.r.t. 
   *
   *  \param pluginPath: the complete path to the plugin
   *  \param functionName: the name of the function to use in this plugin
   */
  void setComputeJacobianFIntvFunction(const std::string &pluginPath,
                                       const std::string &functionName);

  /** set a specified function to compute jacobian following q of the FInt
   *
   *  \param fct a pointer on the plugin function
   */
  void setComputeJacobianFIntqFunction(FInt_NE fct);

  /** set a specified function to compute jacobian following v of the FInt
   *
   *  \param fct a pointer on the plugin function
   */
  void setComputeJacobianFIntvFunction(FInt_NE fct);

  /** allow to set a specified function to compute the jacobian w.r.t q of the
   *  internal forces
   *
   *  \param pluginPath: the complete path to the plugin
   *  \param functionName: the name of the function to use in this plugin
   */
  void setComputeJacobianMIntqFunction(const std::string &pluginPath,
                                       const std::string &functionName);
  /** allow to set a specified function to compute the jacobian following v of
   *  the internal forces w.r.t. 
   * 
   *  \param pluginPath: the complete path to the plugin 
   *  \param functionName: the name of the function to use in this plugin
   */
  void setComputeJacobianMIntvFunction(const std::string &pluginPath,
                                       const std::string &functionName);

  /** set a specified function to compute jacobian following q of the FInt
   *
   *  \param fct a pointer on the plugin function
   */
  void setComputeJacobianMIntqFunction(FInt_NE fct);

  /** set a specified function to compute jacobian following v of the FInt
   *
   *  \param fct a pointer on the plugin function
   */
  void setComputeJacobianMIntvFunction(FInt_NE fct);

  /** function to compute the external forces
   *
   *  \param time the current time
   */
  virtual void computeFExt(double time);

  /** default function to compute the external forces
   *
   *  \param time the current time
   *  \param fExt the computed external force (in-out param)
   */
  virtual void computeFExt(double time, SP::SiconosVector fExt);

  /** function to compute the external moments
   *  The external moments are expressed by default in the body frame, since the
   *  Euler equation for Omega is written in the body--fixed frame. Nevertheless,
   *  if _isMextExpressedInInertialFrame) is set to true, we assume that the
   *  external moment is given in the inertial frame and we perform the rotation
   *  afterwards
   * 
   *  \param time the current time
   *  \param mExt the computed external
   *  moment (in-out param)
   */
  virtual void computeMExt(double time, SP::SiconosVector mExt);

  virtual void computeMExt(double time);

  /** Adds a force/torque impulse to a body's FExt and MExt vectors in
   *  either absolute (inertial) or relative (body) frame.  Modifies
   *  contents of _fExt and _mExt! Therefore these must have been set
   *  as constant vectors using setFExtPtr and setMExtPtr prior to
   *  calling this function.  Adjustments to _mExt will take into
   *  account the value of _isMextExpressedInInertialFrame.
   *
   *  \param force A force vector to be added.
   *  \param forceAbsRef If true, force is in inertial frame, otherwise
   *  it is in body frame.
   *  \param pos A position at which force should be applied.  If nullptr,
   *  the center of mass is assumed.
   *  \param posAbsRef If true, pos is in inertial frame, otherwise it
   *  is in body frame.
   */
  void addExtForceAtPos(SP::SiconosVector force, bool forceAbsRef,
                        SP::SiconosVector pos = SP::SiconosVector(),
                        bool posAbsRef = false);

  void computeJacobianMExtqExpressedInInertialFrameByFD(double time,
                                                        SP::SiconosVector q);
  void computeJacobianMExtqExpressedInInertialFrame(double time,
                                                    SP::SiconosVector q);

  /** default function to compute the internal forces
   *
   *  \param time the current time
   */
  // void computeFInt(double time);

  /** function to compute the internal forces
   *
   *  \param time the current time
   *  \param q
   *  \param v
   */
  void computeFInt(double time, SP::SiconosVector q, SP::SiconosVector v);

  /** default function to compute the internal forces
   * 
   *  \param time the current time
   *  \param q
   *  \param v
   *  \param fInt the computed internal force vector
   */
  virtual void computeFInt(double time, SP::SiconosVector q,
                           SP::SiconosVector v, SP::SiconosVector fInt);

  /** default function to compute the internal moments
   *
   *  \param time the current time
   *  \param q
   *  \param v
   */
  void computeMInt(double time, SP::SiconosVector q, SP::SiconosVector v);

  /** default function to compute the internal moments
   *
   *  \param time the current time
   *  \param q
   *  \param v
   *  \param mInt the computed internal moment vector
   */
  virtual void computeMInt(double time, SP::SiconosVector q,
                           SP::SiconosVector v, SP::SiconosVector mInt);

  /** default function to update the plugins functions using a new time:
   *  
   *  \param time  the current time
   */
  void updatePlugins(double time) override{};

  /** Allocate memory for forces and its jacobian.
   */
  void init_forces() override;

  /** Default function to compute forces
   *
   *  \param time double, the current time
   */
  virtual void computeForces(double time);

  /** function to compute forces with some specific values for q and twist (ie
   *  not those of the current state). \param time double : the current time
   *
   *  \param q SP::SiconosVector: pointers on q
   *  \param twist SP::SiconosVector: pointers on twist
   */
  void computeForces(double time, SP::SiconosVector q, SP::SiconosVector twist) override;

  /** Default function to compute the jacobian w.r.t. q of forces
   *
   *  \param time double, the current time
   */
  void computeJacobianqForces(double time) override;

  /** Default function to compute the jacobian w.r.t. v of forces
   *
   *  \param time double, the current time
   */
  void computeJacobianvForces(double time) override;

  /** function to compute gyroscopic forces with some specific values for q and
   *  twist (ie not those of the current state).
   *
   *  \param twist SP::SiconosVector: pointers on  twist vector
   */
  virtual void computeMGyr(SP::SiconosVector twist);

  /** function to compute gyroscopic forces with some specific values for q and
   *  twist (ie not those of the current state).
   *
   *  \param twist pointer to twist vector
   *  \param mGyr pointer to gyroscopic forces
   */
  virtual void computeMGyr(SP::SiconosVector twist, SP::SiconosVector mGyr);

  /** Default function to compute the jacobian following q of mGyr
   *
   *  \param time the current time
   */
  virtual void computeJacobianMGyrtwist(double time);

  /** Default function to compute the jacobian following q of mGyr
   *  by forward finite difference
   *
   *  \param time the current time
   *  \param q current state
   *  \param twist pointer to twist vector
   */
  virtual void computeJacobianMGyrtwistByFD(double time, SP::SiconosVector q,
                                            SP::SiconosVector twist);

  // /** Default function to compute the jacobian following v of mGyr
  //  *  \param time the current time
  //  */
  // virtual void computeJacobianvForces(double time);

  /** To compute the jacobian w.r.t q of the internal forces
   *
   *  \param time double : the current time
   */
  void computeJacobianFIntq(double time);

  /** To compute the jacobian w.r.t v of the internal forces
   *
   *  \param time double : the current time
   */
  void computeJacobianFIntv(double time);

  /** To compute the jacobian w.r.t q of the internal forces
   *
   *  \param time double
   *  \param position SP::SiconosVector
   *  \param twist SP::SiconosVector
   */
  virtual void computeJacobianFIntq(double time, SP::SiconosVector position,
                                    SP::SiconosVector twist);
  /** To compute the jacobian w.r.t q of the internal forces
   *  by forward finite difference
   *
   *  \param time double
   *  \param position SP::SiconosVector
   *  \param twist SP::SiconosVector
   */
  void computeJacobianFIntqByFD(double time, SP::SiconosVector position,
                                SP::SiconosVector twist);

  /** To compute the jacobian w.r.t. v of the internal forces
   *
   *  \param time double: the current time
   *  \param position SP::SiconosVector
   *  \param twist SP::SiconosVector
   */
  virtual void computeJacobianFIntv(double time, SP::SiconosVector position,
                                    SP::SiconosVector twist);

  /** To compute the jacobian w.r.t v of the internal forces
   *  by forward finite difference
   *
   *  \param time double
   *  \param position SP::SiconosVector
   *  \param twist SP::SiconosVector
   */
  void computeJacobianFIntvByFD(double time, SP::SiconosVector position,
                                SP::SiconosVector twist);

  /** To compute the jacobian w.r.t q of the internal forces
   *
   *  \param time double : the current time
   */
  virtual void computeJacobianMIntq(double time);

  /** To compute the jacobian w.r.t v of the internal forces
   *
   *  \param time double : the current time
   */
  virtual void computeJacobianMIntv(double time);

  /** To compute the jacobian w.r.t q of the internal forces
   *
   *  \param time double : the current time,
   *  \param position SP::SiconosVector
   *  \param twist SP::SiconosVector
   */
  virtual void computeJacobianMIntq(double time, SP::SiconosVector position,
                                    SP::SiconosVector twist);

  /** To compute the jacobian w.r.t q of the internal moments
   *  by forward finite difference
   *
   *  \param time double
   *  \param position SP::SiconosVector
   *  \param twist SP::SiconosVector
   */
  void computeJacobianMIntqByFD(double time, SP::SiconosVector position,
                                SP::SiconosVector twist);

  /** To compute the jacobian w.r.t. v of the internal forces
   *
   *  \param time double: the current time
   *  \param position SP::SiconosVector
   *  \param twist SP::SiconosVector
   */
  virtual void computeJacobianMIntv(double time, SP::SiconosVector position,
                                    SP::SiconosVector twist);

  /** To compute the jacobian w.r.t v of the internal moments
   *  by forward finite difference
   *
   *  \param time double
   *  \param position SP::SiconosVector
   *  \param twist SP::SiconosVector
   */
  void computeJacobianMIntvByFD(double time, SP::SiconosVector position,
                                SP::SiconosVector twist);

  virtual void computeT();

  virtual void computeTdot();

  ACCEPT_STD_VISITORS();
};
#endif // NEWTONEULERNLDS_H
